#ifndef GEOMBRIDGE_HPP
#define GEOMBRIDGE_HPP
#include "Data/Data.hpp"
#include "Models/Likelihood.hpp"
//#include "Gibbs.hpp"
#define _EPS 0.01
#define NB_K  10 
#define P_K 1 
#define _Pr 0.01 
#define MARG 0
#define MAXITK 100 
#define MAXACC 0.4
#define MINACC 0.15
#define THRES 0.0001 
template<class Kernel, class Resample>
class Particle<Kernel, Resample, Density::GeomBridge>{

public:
	Particle(Kernel *K,Resample *R,Density::GeomBridge *D,int M, Distribution::Distribution *F,double C){
		_p=(*D).Get_p();
		mat Xt(M,_p);//p le nombre de parametre a obtenir de Density
		W=new double[M];
		X=Xt;
		_K=K;
		_R=R;
		_F=F;
		_Z=0;
		_C=C;
		_boo=0;
		_D=D;
		kk=-1;
		_thres=C*M;
		_M=M;
		Normalize();
		y=X.t();
		_n=(*D).Get_n();
		O2=new Data<ofstream>("Tempering.txt");	
		_K->Set_n(M);
	//	cout << "///" << _n << "///";
	}
	Particle(){
		cout << "Hidee2!";
	}
	Particle(const Particle& X)
	{
		cout << "Hidee!";
	}
	~Particle(){
	//	delete[] W;
	}
	void Filter()
	{
		this->Init();
		int i=1;
		double p=0; 
		//for(int i=1;i<(_n);i++)
	//	i{
		double b=(*_D).Get_bn();
	//	b+=(double)(1/(double)(_P-1));
		_temp=StepLength(b);
		Phiv.push_back(_temp);
		b+=_temp;
		cout << "b: " << b << " p";
		_D->Set_Phi(b);
		y=X.t();
		_D->Weight(y,W,i,0);//le p designera comment on avance 
		double ess=Ess_W(W,1);
		cout << " ESS: " << ess;
		X=y.t();
		p=_D->Get_bn();
		while(p<1){
	//		cout << i;
			cout << " step: " << i << "\n";
			this->Step(i);
			p=_D->Get_bn();
		}

	//	Correction2();
		(*O2).Close();
		
	}
	void Init()
	{
		//pior
		X=_F->scrambled(_M);	
		Write();
	}
	void Write()
	{
		for(int i=0;i<_M;i++)
		{
			std::ostringstream oss;
			oss << X(i,MARG);
			string s=oss.str();
			(*O2).Write(s);
			(*O2).Write(" ");

		}
			(*O2).Write("\n");
	}
	void Step(int i)
	{
		//cout << "Ess:" << Ess << "\n";
				//cout << "\\";
//		if(Ess< _thres)
//		{
		/////Resample
			mat v=growingvect(_M);
			double *w=new double[_M];
			double sum=W[0];
			for(int ii=1;ii<_M;ii++)
			{
				if(exp(W[ii])!=0)
				{
					double t=log_add(W[ii],sum);
					sum=t;
				}
			}
			cout << "sum " <<sum;
			for(int j=0;j<_M;j++){
				 w[j]=exp(W[j]-sum);
			}
			(*_R)(&_M,w,v);
			delete[] w;
			
			Arangemat(X,v);
			Normalize();//poids fixé a 1
			Write();
			//mat C=cov(X);
			//cout << C;
			//cout << "sum: "  << sum(X,0) << "\n";
			//cout << "before:" << X.row(1);
	//		for(int k=0;k<3;k++)
	//		{	
		//Move	
			cout << "\nmean " << mean(X,0) << "\n";
		//Reinitialize param
			_K->Set_bool(1);// bool indicates to class Kernel that this is the first iteration 
			double prop_moved=0;
			int kk=0;
			double acc=1;
			mat X0=X;
		//Set variance
			_K->Set_s(X);

		//First MOve
			_K->testMove(&(X),i);
			//_K->Move2(&(X),i);
			acc=_K->Get_accept();
			
/////////////////////////////////////////////////////////////////////////////////////
			//cout << "Distance "  <<dd1 << "\n";
		//Set Second and following Moove
			_K->Set_bool(0);
		//	while(sqrt(pow(dd1-dd0,2))>THRES) 
			//while(prop_moved<P_K & kk<MAXITK)
			for(int o=0;o<4;o++)
			{	
				
				if(acc>0.15)
				{
					_K->Set_s(X);
					_K->Move(&(X),i);
					prop_moved=_K->nMove();
					//	cout << dd1;
					acc=_K->Get_accept();
				}else{
					_K->Set_Sigma(0.25);
					//_K->Set_s(X);
					cout << "Hello";
					_K->Move(&(X),i);
					prop_moved=_K->nMove();
					_boo=1;
				}
				kk++;
			//	cout << "\nmean " << mean(X,0) << "\n";
			}
			prop_moved=_K->nMove();
			Acc.push_back(_K->Get_accept());
	
			cout << "\nmean " << mean(X,0) << "\n";
			kk++;	
		/*	if(Acc[kk]<0.15)
			{
				_K->Set_Sigma(0.75);
			}else if(Acc[kk]>0.45){
			//	cout << "Hello\n" << Acc[Acc.size()];
				_K->Set_Sigma(1.5);
			}	
			*/
			
			//		}
			//cout << "after:" << X.row(1);

//		}
				
		y=X.t();
		//cout << X << "////";	
		double b=_D->Get_bn();
	//	b+=(double)(1/(double)(_P-1));
		//double etas=Ess_W(_D->Eta_star(_temp,W,y,1),0)/_M;
		//cout << "etas :"<< etas;
	//	if(etas>_C+_EPS | etas<_C-_EPS)
	//	{
			_temp=StepLength(b);
//		}else{
//			_temp=min(_temp,1-b);
//		}
		Phiv.push_back(_temp);
		b+=_temp;
		cout << "b: " << b << "p";
		_D->Set_Phi(b);
		
		//dans le cas de tempering peut etre determiner
		_D->Weight(y,W,i,0);//le p designera comment on avance 
	
	
		
		//	cout << "after:" << y.col(1);
		X=y.t();
		double ess=Ess_W(W,1);
		cout << " ESS: " << ess;
		//cout << " ESS: " << Ess_W(W,1);
	}
	void Correction(void)
	{
		mat v=growingvect(_M);
		double *w=new double[_M];
		for(int j=0;j<_M;j++){
			 w[j]=exp(W[j]);
		//	cout << w[j] << "\n";
		}
		(*_R)(&_M,w,v);
		delete[] w;
		//cout << v << "\n";
		cout << "\nmean " << mean(X,0) << "\n";
		Arangemat(X,v);
		Normalize();
		_K->Set_bool(1);
		double prop_moved=0;
		int kk=0;
		double acc=1;
		_K->Set_s(X);
		mat Z=X;
		mat X0=X;
		_K->Move(&(Z),1);
		acc=_K->Get_accept();
/*
		while(acc<0.15 |acc>MAXACC)
		{
			if(acc<0.15)
			{
				_K->Set_Sigma(0.5);
			}else if(acc>MAXACC){
		//	cout << "Hello\n" << Acc[Acc.size()];
				_K->Set_Sigma(1.5);
			}
			cout << "....";
			try{
				_K->Set_s(X);
			
			}
			catch(std::exception& e)
			{
				cout << "chol did not converge, this step is not adaptative";
				
			}
			Z=X;
			_K->Move(&(Z),1);

			acc=_K->Get_accept();
			//cout << "acc " << acc;
			
		}
	*/	
		X=Z;
		double dd0=0;
		double dd1=Distance(X0,X);
		cout << "Distance "  <<dd1 << "\n";
		_K->Set_bool(0);
		//while(sqrt(pow(dd1-dd0,2))>THRES) 
		//while(prop_moved<P_K & kk<MAXITK)
		for(int o=0;o<1;o++)
		{	
			_K->Move(&(X),1);
			_K->Set_bool(0);
			dd0=dd1;
			dd1=Distance(X0,X);
			prop_moved=_K->nMove();
			cout << dd1;
			kk++;
		}


	}

	double Ess_W(double *w, int tf)//renormalisation???
	{
		double *foo= new double[_M];
		double *foo2= new double[_M];
		double sum=w[0];
		double sum2=exp(w[0]);
		for(int i=1;i<_M;i++)
		{
			if(exp(w[i])!=0)
			{
				double t=log_add(w[i],sum);
				sum=t;
				sum2+=exp(w[i]);
			}
		}
		//cout << "sum " << sum-log(sum2);

		if(tf){
			_Z+=sum-log(_M);
			cout << "Z "<< _Z << "\n";
		}
		//cout << "the sum "<< sum2;
		
		for(int j=0;j<_M;j++)
		{
			foo[j]=(w[j]-sum);
			foo2[j]=exp(w[j])/sum2;
			//cout << exp(foo[j])-exp(w[j])/sum2 << "\n";
		}

		sum=2*foo[0];
		sum2=foo2[0]*foo2[0];
		for(int i=1;i<_M;i++)
		{
			double t=log_add(sum,2*foo[i]);
			sum=t;
			sum2+=foo2[i]*foo2[i];
			//cout << sum2 << "\n";	
		}
		delete[] foo;
		//delete[] foo2;
		//cout << "logESS "<< -sum;
		if(sum!=sum){
			return (double)1/sum2;
		}else{
			return exp(-sum);		
		}
	}
	double StepLength(double phi)
	{
		double u=10-phi;
		double l=0;
		double thres=0.00001;
		double e=2;
		double alpha=0.05;
		double eta=0;	
		int Bool=1;
		while((e>thres)&(l<1-phi))
		{
			mat y=X.t();
		//	cout << y; 
		/*	for(int i=0;i<_M;i++)
			{
			     	cout << W[i] << "\n";
			}*/
			eta=Ess_W(_D->Eta_star(alpha,W,y,Bool),0)/_M;
			if(eta>_C)
			{
				l=alpha;
				alpha=(double)(alpha+u)/2;
			}else{
				u=alpha;
				alpha=(double)(alpha+l)/2;

			}	
			e=abs<double>(u-l);
			cout << "\\\\\\ "<< eta <<" \\\\\\\\\n"; 
			Bool=0;
		}
		return min2<double>(alpha,1-phi);
	}
	double Distance(mat X0, mat Xi)
	{
		double mean=0;
		for(int i=0;i<_M;i++)
		{
			mean+=Norm<rowvec>(X0(i,span::all),Xi(i,span::all));			
		}
		return mean/_M;
	}

	
	mat Get_theta(void){return X;}
	void Normalize(void){
		for(int i=0;i<_M;i++){
			W[i]=0;
		}
	}
	mat   Get_W(void){
		double sum=W[0];
		mat C(_M,1);
		for(int i=1;i<_M;i++)
		{
			double t=log_add(sum,W[i]);
			sum=t;
		}	
		for(int i=0;i<_M;i++)
		{
			C(i,0)=exp((W[i]-sum)+log(_M));
		}
		return C;
	}
	double Get_Z(void){
		cout << "ev: " << _Z;       
		return _Z;}
	mat Get_Phiv(void)
	{
		int n=Phiv.size();
		mat res(n,1);
		for(int i=0;i<n;i++)
		{
			res(i,0)=Phiv[i];
		}
		return res; 
	}
	mat Get_Acc(void)
	{
		int n=Acc.size();
		mat res(n,1);
		for(int i=0;i<n;i++)
		{
			res(i,0)=Acc[i];
		}
		return res; 
	}

void Correction2(void)
{
	mat v=growingvect(_M);
	double *w=new double[_M];
	for(int j=0;j<_M;j++){
		 w[j]=exp(W[j]);
	//	cout << w[j] << "\n";
	}
	(*_R)(&_M,w,v);
	delete[] w;
	//cout << v << "\n";
	cout << "\nmean " << mean(X,0) << "\n";
	Arangemat(X,v);
	Normalize();
	_K->Set_bool(1);
	double prop_moved=0;
	int kk=0;
	double acc=1;
	_K->Set_s(X);
	mat bar(_p,_p);
	bar.fill(10);
	mat s=diagmat(bar);
	Gibbss TdG(s(0,0),_D->Get_X(),_D->Get_Y(),(double)1);
	
	for(int i=0;i<_M;i++)
	{
		for(int j=0;j<4;j++)
		{
			mat z=TdG.rz(X(i,span::all));
			X(i,span::all)=(TdG.Pi_b(z)).t();	
		}
	}

}

private:
	mat X;//value particle
	double *W;//weigths
	double Ess;
	int _M;
	double _thres;
	double _Z;
	Kernel *_K;
	Resample *_R;
	Density::GeomBridge *_D;	
	Distribution::Distribution *_F;
	mat y;
	double _temp;
	int _n;
	int _p;
	int kk;
	Data<ofstream> *O2;
	vector<double> Phiv;
	vector<double> Acc;
	int _boo;
	double _C;
};



#endif
